www.gusucode.com > gpsoft 的惯性导航工具箱源码程序 > matlab代做 修改 程序 gpsoft 的惯性导航工具箱/惯导工具箱/navupd2.m

    function [DCMel, DCM_ll_E] = navupd2(omega1_el_L,omega2_el_L,td12,DCMel,procflg)
%NAVUPD2      Update the direction cosine matrix relating the 
%              local-level frame relative to the earth frame
%
%             This is another version of NAVUPDAT.  In this case the
%             local-level-to-earth DCM is updated and then converted
%             to earth-to-local-level DCM.
%
%	[DCMel, DCM_ll_E] = navupd2(omega1_el_L,omega2_el_L,td12,DCMel,procflg)
%
%   INPUTS
%       omega2_el_L = craft-rate vector at current time
%       omega1_el_L = craft-rate vector at previous position update
%       td12 = time difference (in seconds) between time indices 1 and 2
%              (this is a positive number; i.e., td12 = time2 - time1)
%       DCMel = 3x3 direction cosine matrix providing the
%             transformation from the earth frame
%             to the local-level (ENU) frame
%       procflg = processing flag; 0=first order approximation; 1=exact solution
%
%   OUTPUTS
%       DCMel = updated earth-to-local-level direction cosine matrix
%       DCM_ll_E = direction cosine matrix relating the local-level frame
%                  at the end of the update interval to the local-level
%                  frame at the beginning of the update interval
%                  (relative to the earth frame)
%

%  REFERENCE
%      Savage, P., "Strapdown Inertial Navigation Integration Algorithm
%      Design Part 2: Velocity and Position Algorithms," AIAA Journal of
%      Guidance, Control, and Dynamics, Vol. 21, No. 2, March-April 1998.
%
%	M. & S. Braasch 8-98
%	Copyright (c) 1998 by GPSoft LLC
%	All Rights Reserved.
%

if nargin<5,error('insufficient number of input arguments'),end
C = [0 1 0; 1 0 0; 0 0 -1];    % conversion between NED and ENU

omega_avg = 0.5*( omega1_el_L + omega2_el_L );
ang_vect = omega_avg*td12;
S = skewsymm(ang_vect);
if procflg == 0,
   DCM_ll_E = eye(3) + S;   % First order approximation
else
   magn = norm(ang_vect);
   if magn == 0,
      A = eye(3);
   else                   % Exact solution
      A = eye(3) + (sin(magn)/magn)*S + ( (1-cos(magn))/magn^2 )*S*S;
   end
   DCM_ll_E = A;
end
DCMel = ( (DCMel')*DCM_ll_E )';